//默认引用：
#include "Drv_OpenMV.h"
#include "Ano_ProgramCtrl_User.h"
#include "User_Program.h"

//设定
#define OPMV_OFFLINE_TIME_MS  1000  //毫秒

//全局变量
u16 offline_check_time;
u8 openmv_buf[20];
_openmv_data_st opmv;
unsigned char message[] = "OK";

/**********************************************************************************************************
*函 数 名: OpenMV_Byte_Get
*功能说明: OpenMV字节数据获取//判断帧头与帧尾校验和
*参    数: 字节数据
*返 回 值: 无
**********************************************************************************************************/
/* 
数据包协议： 0xaa   载荷长度   载荷1 载荷2 载荷3  ...    0xff   ( 0xaa 0x03 0x05 0x06 0x07 0xff ) 
*/
static uint8_t chk_sum=0;
void OpenMV_Byte_Get(u8 bytedata)
{
    static u8 len = 0,rec_sta;
    u8 check_val=0;
    
    //
    openmv_buf[rec_sta] = bytedata;
    //
        if(rec_sta==0)
            {
                if(bytedata==0xaa)
                {
                    rec_sta++;
                }
                else
                {
                    rec_sta=0;
                }
            }
            

            else if(rec_sta==1)
            {
                //
                len = bytedata;
                if(len<25)
                {
                    rec_sta++;
                }        
                else
                {
                    rec_sta=0;
                }
            }
            else if(rec_sta==(len+2))//帧尾
            {
                for(int i=0;i<=rec_sta;i++){
                chk_sum+=openmv_buf[i];
                }
                if(bytedata==chk_sum)
                {
                    //解析成功
                    /*****这里写解析函数*******/
                    OpenMV_Data_Analysis(openmv_buf,len+2);
                    chk_sum=0;
                    rec_sta=0;
                }
            }
            else
            {
                rec_sta++;
            }   
}


/**********************************************************************************************************
*函 数 名: OpenMV_Data_Analysis
*功能说明: OpenMV数据解析
*参    数: 缓存数据（形参），长度
*返 回 值: 无
**********************************************************************************************************/
#include "Ano_DT.h"
static void OpenMV_Data_Analysis(u8 *buf_data,u8 len)
{
	if(*(buf_data+3)==0x41)
	{
		opmv.cb.color_flag = *(buf_data+5);
		opmv.cb.sta = *(buf_data+6);
		opmv.cb.pos_x = (s16)((*(buf_data+7)<<8)|*(buf_data+8));
		opmv.cb.pos_y = (s16)((*(buf_data+9)<<8)|*(buf_data+10));
		opmv.cb.dT_ms = *(buf_data+11);
		//		
		opmv.mode_sta = 1;
 }
	
	else if(*(buf_data+3)==0x42)
	{
		opmv.lt.sta = *(buf_data+5);
		opmv.lt.angle = (s16)((*(buf_data+6)<<8)|*(buf_data+7));
		opmv.lt.deviation = (s16)((*(buf_data+8)<<8)|*(buf_data+9));
		opmv.lt.p_flag = *(buf_data+10);
		opmv.lt.pos_x = (s16)((*(buf_data+11)<<8)|*(buf_data+12));
		opmv.lt.pos_y = (s16)((*(buf_data+13)<<8)|*(buf_data+14));
		opmv.lt.dT_ms = *(buf_data+15);
		//
		opmv.mode_sta = 2;
	}
	
	else   
		{
		opmv.cb.color_flag = 1;
		opmv.cb.pos_x = (s16)((*(buf_data+2)<<8)|*(buf_data+3));
		opmv.cb.pos_y = (s16)((*(buf_data+4)<<8)|*(buf_data+5));
		opmv.cb.id    = (s16)((*(buf_data+6)<<8)|*(buf_data+7));
		//opmv.cb.dT_ms = *(buf_data+8);	
			
		opmv.mode_sta = 1;  //3:读取色块及定位模式
	  }
	//
	OpenMV_Check_Reset();
}

/**********************************************************************************************************
*函 数 名: OpenMV_Offline_Check
*功能说明: OpenMV掉线检测，用来检测硬件是否在线
*参    数: 时间（毫秒）
*返 回 值: 无
**********************************************************************************************************/
void OpenMV_Offline_Check(u8 dT_ms)
{
	if(offline_check_time<OPMV_OFFLINE_TIME_MS)
	{
		offline_check_time += dT_ms;
	}
	else
	{
		opmv.offline = 1;
		opmv.mode_sta = 0;
	}
	
}

/**********************************************************************************************************
*函 数 名: OpenMV_Check_Reset
*功能说明: OpenMV掉线检测复位，证明没有掉线
*参    数: 无
*返 回 值: 无
**********************************************************************************************************/
static void OpenMV_Check_Reset()
{
	offline_check_time = 0;
	opmv.offline = 0;
}



//openmv定点
#define CV_KP 0.01f
void OpenMV_P_ctrl()
{
	float x_out=CV_KP*opmv.cb.pos_x;
	float y_out=CV_KP*opmv.cb.pos_y;
	Program_Ctrl_User_Set_HXYcmps(x_out,y_out);
}


//读取色块id
//7位数据
uint8_t Color_block_input_id()
{
	return (u8)(opmv.cb.id);
}



